#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>

ros::Publisher pub;

/*
- header：消息头，包含时间戳和坐标系信息。
- angle_min：扫描起始角度。
- angle_max：扫描终止角度。
- angle_increment：角度分辨率。
- time_increment：时间分辨率。
- scan_time：扫描时间。
- range_min：最小测量距离。
- range_max：最大测量距离。
- ranges：测量距离数组。
- intensities：激光强度数组。
 */
void scanCallback(const sensor_msgs::LaserScan::ConstPtr &scan) {
    // 获取激光点的个数
    int num_points = (scan->angle_max - scan->angle_min) / scan->angle_increment;
    ROS_INFO_STREAM("Number of points: " << num_points);

    // 获取第一个激光点的距离
    float first_range = scan->ranges[0];
    ROS_INFO_STREAM("First range: " << first_range);

    // 获取最后一个激光点的距离
    float last_range = scan->ranges[num_points - 1];
    ROS_INFO_STREAM("Last range: " << last_range);


    // 获取前方180度激光点的个数
    int num_points_180 = 180.0 / scan->angle_increment;
    ROS_INFO_STREAM("Number of points in 180 degrees: " << num_points_180);

    // 获取前方180度激光点的距离
    std::vector<float> ranges_180(scan->ranges.begin() + (num_points - num_points_180) / 2,
                                  scan->ranges.begin() + (num_points + num_points_180) / 2);

    // 将不在前180度范围内的激光点的距离设置为infinite
    for (int i = 0; i < (num_points - num_points_180) / 2; i++) {
        ranges_180.insert(ranges_180.begin(), std::numeric_limits<float>::infinity());
        ranges_180.push_back(std::numeric_limits<float>::infinity());
    }

    // 发布前方180度激光点的距离
    sensor_msgs::LaserScan scan_180 = *scan;
    scan_180.ranges = ranges_180;
    scan_180.angle_min = -M_PI / 2.0;
    scan_180.angle_max = M_PI / 2.0;
    scan_180.angle_increment = scan->angle_increment;
    scan_180.time_increment = scan->time_increment;
    scan_180.scan_time = scan->scan_time;
    scan_180.range_min = scan->range_min;
    scan_180.range_max = scan->range_max;

    // 发布前方180度激光点的距离
    pub.publish(scan_180);
}

int main(int argc, char **argv) {
    // 初始化ROS节点
    ros::init(argc, argv, "cre_laser_scan_listener");

    // 创建节点句柄
    ros::NodeHandle n;

    // 创建激光点的发布者
    pub = n.advertise<sensor_msgs::LaserScan>("/scan_180", 1000);

    // 订阅/scan话题
    ros::Subscriber sub = n.subscribe("/scan", 1000, scanCallback);

    // 循环等待回调函数
    ros::spin();

    return 0;
}